
%% ========================================================================
% VTP PATROL AERODYNAMIC COEFFICIENT TABLEAU
%==========================================================================

clc;

%% ========================================================================
% Useful Constants
%==========================================================================

d2r     = 57.3;
g       = 9.81;
m2ft    = 3.28084;
Kg2slug = 0.0685218;
Kg2lb   = 2.20462;

%% ========================================================================
% Atmospheric Constants
%==========================================================================

T0      = 288.16;                 % Temp. at Sea Level [K]
rho0    = 1.225;                  % Density [Kg/m^3]
L       = 0.0065;                 % Lapse Rate [K/m]
R       = 287.26;                 % Gas Constant J/Kg/K
gam     = 1.403;                  % Ratio of Specific Heats
P0      = 101325.0;               % Pressure at Sea Level [N/m^2]
h_trop  = 11000.0;                % Height of Troposphere [m]


%% ========================================================================
% Aircraft Configuration
%==========================================================================

S_ref    = 1.49;                    % Reference area   [m^2]
cbar     = 0.2973;                  % wing's mean chord[m]
%d_ref    = 1.297;                  % Reference length [m]
d_ref    = cbar;                    % Reference length [m]
b_ref    = 3.25;                    % Reference Span   [m]
massBody = 16.462;                  % Empty mass       [m]
massFuelLoad = 4.5;                 % Fuel and load    [m]
mass     = massBody+massFuelLoad;   % Total Mass of the vehicle[Kg]
Iyy    = 4.2923;                       % Inertias determined using Empty Mass
Ixx    = 1.9221;                       % Inertia Units are [kg.m2]
Izz    = 2.9076;                       %
Ixz    = 0.0;                          % symmetric aircraft
Inertia = diag([Ixx Iyy Izz]);      % Matrix of inertia
x_cg = 1.07;                        % x_cor of gravity center [m]
z_cg = -0.014;                      % z_cor of gravity center [m]
x_ref = 1.14;                       % x_cor of center of pressure [m]

%% ========================================================================
% Component configuration
%==========================================================================

e        = 0.75;                    % Span efficiency factor
S_wing   = 1.0075;                  % Area of wing   [m^2]
AR_wing  = 10.4844;                 % Aspect ratio of wing
cg_wing  = -0.0106;                 % Distance aero center back from CG [m]
S_vtail  = 0.05471*2;               % Area of ver_tail [m^2]
eta_vtail= 1.0;                     % Wake factor for vertail
cg_vtail = 0.9839;                  % Distance tail AC back from CG, AAA[m]
S_tail   = 0.13;                    % Area of tail [m^2]
eta_tail = 0.931;                   % Wake factor for htail
cg_tail  = 0.9839;                  % Distance tail AC back from CG [m]

%% ========================================================================
% Initial Conditions
%==========================================================================

% load VTP_glideslope
Aileron_Position  = 0.0;
Elevator_Position = 0.0;
Rudder_Position   = 0.0;
Flap_Position     = 0.0;
Clock_sec         = 0.0;

%Initial State Conditions: Radians and Re-named
Roll_rate         = 0.0;            %Roll rate [rad s-1]
Pitch_rate        = 0.0;            %Pitch rate [rad s-1]
Yaw_rate          = 0.0;            %Yaw Rates
Altitude_Actual   = 300.0;          %[m]
Velocity          = 37.74;          %[m/s]
FlightPath_Angle  = 0.0;            %[rad]
Sideslip_Angle    = 0/d2r;          %[rad]
AoA               = 1./d2r;         %[rad]
Bank_Angle        = 0.0;            %[rad]
Heading_Angle     = 0.0;            %[rad]
North_Position    = 0.0;            %[m]
East_Position     = 0.0;            %[m]

% Initial position
Xme_0 = [North_Position; East_Position; -Altitude_Actual];

% Initial airspeed, AoA, Sideslip angle
Vmw  = [Velocity; AoA; Sideslip_Angle];

% Initial wind orientation
Wangle = [Bank_Angle; FlightPath_Angle; Heading_Angle];

% Initial body rotation rate
pqr0 = [Roll_rate; Pitch_rate; Yaw_rate];

% Initial body euler orientation(roll,pitch,yaw)
euler_0 = [0/d2r;AoA/d2r;0];

% Initial velocity in body axe
Vu = Velocity*cos(AoA)*cos(Sideslip_Angle);
Vv = Velocity*cos(AoA)*sin(Sideslip_Angle);
Vw = Velocity*sin(AoA);
Uvw0 = [Vu; Vv; Vw];

%% ========================================================================
% Flight condition
%==========================================================================

Mach = 0.062;
altitude = 300.0;
alpha = [-6 -5 -4 -3 -2.5 -2 -1.5 -1 -0.5 0 1 2 3 4 5 6 7 8 9 10];
nmach     = length(Mach);
naltitude = length(altitude);
nalpha    = length(alpha);
alpha_vec_0 = alpha;

%% ========================================================================
% Aircraft Aerodynamic Coefficients
%==========================================================================

% longitudinal stab deriv.
CD = [ .027 .025 .023 .022 .022 .022 .023 .023 .023 .024 .026 .028 .032...
     .036 .040 .046 .052 .059 .067 .075];
CL = [-.253 -.183 -.113 -.046 -.012 .022 .056 .090 .125 .159 .230 .302...
    .374 .447 .520 .594 .668 .744 .819 .893];
Cm = [ .1536 .1282 .1030 .0777 .0651 .0524 .0397 .0270 .0142 .0014...
    -.0245 -.0509 -.0775 -.1032 -.1282 -.1541 -.1826 -.2143 -.2474...
    -.2786];

CLad = [1.049 1.047 1.040 1.035 1.039 1.050 1.059 1.069 1.078 1.087 ...
    1.100 1.070 1.019 .9875 .9815 1.004 1.042 1.073 1.079 1.076];
Cmad = 1.0*[-3.706 -3.698 -3.675 -3.655 -3.671 -3.708 -3.742 -3.775 -3.807...
    -3.839 -3.887 -3.780 -3.602 -3.489 -3.467 -3.547 -3.682 -3.791...
    -3.811 -3.801];

CLq = 5.752*ones(1,20);     % per rad
Cmq = -1.038*ones(1,20);    % per rad, check Digital DATCOM
% Sideforce and roll stability derivatives
% - per deg for natural angle (alpha,beta)
% - per rad for derivatives (alphadot, betadot,p,q,r)


%CYB    = -4.4e-3;           %per deg, Datcom - commented out 2/10/12 DTP
CYB    = -8.195e-3;          %per deg - AA output
CYbeta = CYB*ones(1,20);
% CYp  = 8.88e-2*ones(1,20);  %per rad
CYp  = [-4.521E-04 -1.611E-03 -2.715E-03 -3.766E-03 -4.275E-03...
    -4.797E-03 -5.331E-03 -5.877E-03 -6.434E-03 -7.002E-03 -8.169E-03...
    -9.374E-03 -1.061E-02 -1.188E-02 -1.317E-02 -1.448E-02 -1.581E-02...
    -1.716E-02 -1.852E-02 -1.983E-02];

CYr = .1824*ones(1,20);     %per rad
CnB = 9.57e-4;            %per deg, Datcom

% Cnp = [1.956E-02 1.371E-02 7.944E-03 2.257E-03 -5.646E-04 -3.412E-03...
%     -6.284E-03 -9.180E-03 -1.210E-02 -1.504E-02 -2.097E-02 -2.699E-02...
%     -3.308E-02 -3.923E-02 -4.544E-02 -5.171E-02 -5.802E-02 -6.438E-02...
%     -7.088E-02 -7.732E-02]; %-- commented out 2/10/12
Cnp = -.0335*ones(1,20);

% Cnr = [-2.828E-02 -2.869E-02 -2.902E-02 -2.927E-02 -2.937E-02 -2.944E-02...
%     -2.950E-02 -2.954E-02 -2.956E-02 -2.955E-02 -2.948E-02 -2.932E-02...
%     -2.906E-02 -2.871E-02 -2.825E-02 -2.770E-02 -2.706E-02 -2.631E-02...
%     -2.547E-02 -2.454E-02]; %-- commented out 2/10/2012
Cnr = -.0622*ones(1,20);

% Clp = [-3.474E-01 -3.406E-01 -3.335E-01 -3.270E-01 -3.275E-01 -3.314E-01...
%     -3.354E-01 -3.393E-01 -3.429E-01 -3.464E-01 -3.529E-01 -3.587E-01...
%     -3.638E-01 -3.682E-01 -3.718E-01 -3.746E-01 -3.767E-01 -3.780E-01...
%     -3.754E-01 -3.693E-01];
Clp = -.2983*ones(1,20); % AAA checked

ClB = [-1.559E-02 -1.686E-02 -1.810E-02 -1.930E-02 -1.988E-02 -2.047E-02...
    -2.107E-02 -2.167E-02 -2.229E-02 -2.291E-02 -2.417E-02 -2.547E-02...
    -2.680E-02 -2.816E-02 -2.954E-02 -3.094E-02 -3.235E-02 -3.377E-02...
    -3.520E-02 -3.659E-02]/d2r;
% Clr = [-3.559E-02 -2.222E-02 -9.114E-03 3.687E-03 1.000E-02 1.640E-02...
%     2.287E-02 2.943E-02 3.605E-02 4.275E-02 5.635E-02 7.019E-02...
%     8.424E-02 9.847E-02 1.128E-01 1.273E-01 1.419E-01 1.565E-01...
%     1.712E-01 1.855E-01];
Clr0 =  -0.0655;
Clr = Clr0*ones(1,20);

EPSLON = [-2.770 -1.991 -1.215 -.445 .321 1.107 1.919 2.725 3.533 4.338...
    5.113 5.801 6.360 6.834 7.136 6.875 5.397 3.933 2.164 1.259];
DEPSLON = [ .389 .389 .387 .384 .388 .399 .405 .404 .403 .395 .366 .312...
    .258 .194 .010 -.435 -.736 -.808 -.668 -.452];
alpha_vec_xcp = [0 2 4 6 8 10];
XCP = [.063 .063 .063 .063 .063 .063 .021 -.005 -.035 -.060 -.082 -.082...
      -.082 -.082 -.082 -.082 -.082 -.082 -.082 -.082];

%% ========================================================================
% Aircraft Control Device's stability coefficients
%==========================================================================

% Elevator Effect - per deg
delta_elevator_vec = [-20.0 -10.0 0.0 10.0 20.0];
DCLde = 3.5e-3;
DCLde = [-.063 -.039 .000 .039 .063];
DCmde = -7.3e-3;
DCmde = [.1792 .1117 -.0001 -.1117 -.1792];
DCDde = [3.06E-03  1.51E-03 -8.27E-07 -1.44E-04  4.18E-04
     2.57E-03  1.20E-03 -5.20E-07  1.63E-04  9.08E-04
     2.08E-03  9.00E-04 -2.16E-07  4.67E-04  1.39E-03
     1.61E-03  6.02E-04  8.25E-08  7.66E-04  1.87E-03
     1.14E-03  3.08E-04  3.76E-07  1.06E-03  2.34E-03
     6.72E-04  1.53E-05  6.69E-07  1.35E-03  2.80E-03
     2.03E-04 -2.79E-04  9.64E-07  1.65E-03  3.27E-03
     -2.73E-04 -5.78E-04  1.26E-06  1.95E-03  3.75E-03
     -7.71E-04 -8.90E-04  1.57E-06  2.26E-03  4.25E-03
     -1.31E-03 -1.23E-03  1.91E-06  2.60E-03  4.79E-03
     -1.90E-03 -1.60E-03  2.28E-06  2.96E-03  5.37E-03
     -2.53E-03 -1.99E-03  2.68E-06  3.36E-03  6.00E-03
     -3.30E-03 -2.48E-03  3.16E-06  3.84E-03  6.78E-03
     -4.44E-03 -3.19E-03  3.88E-06  4.56E-03  7.92E-03
     -5.93E-03 -4.13E-03  4.81E-06  5.49E-03  9.41E-03
     -7.28E-03 -4.97E-03  5.66E-06  6.34E-03  1.08E-02
     -8.45E-03 -5.71E-03  6.38E-06  7.07E-03  1.19E-02
     -9.53E-03 -6.38E-03  7.06E-06  7.75E-03  1.30E-02
     -1.05E-02 -7.00E-03  7.67E-06  8.36E-03  1.40E-02
     -1.12E-02 -7.44E-03  8.12E-06  8.80E-03  1.47E-02];

% Flap Effect - per deg
delta_flaps_vec = [-20 -10 0 10 20];
DCLdf = 5.3e-3;
DCmdf = -2.5e-3;
DCDdf = [4.00E-03  1.96E-03 -1.55E-06 -1.14E-03 -1.47E-03
    2.69E-03  1.21E-03 -8.05E-07 -3.98E-04 -1.52E-04
    1.38E-03  4.69E-04 -6.05E-08  3.47E-04  1.16E-03
    6.32E-05 -2.76E-04  6.84E-07  1.09E-03  2.48E-03
    -1.25E-03 -1.02E-03  1.43E-06  1.84E-03  3.79E-03
    -2.56E-03 -1.77E-03  2.17E-06  2.58E-03  5.10E-03
    -3.88E-03 -2.51E-03  2.92E-06  3.32E-03  6.42E-03
    -5.19E-03 -3.25E-03  3.66E-06  4.07E-03  7.73E-03
    -6.51E-03 -4.00E-03  4.41E-06  4.81E-03  9.05E-03
    -7.82E-03 -4.74E-03  5.15E-06  5.56E-03  1.04E-02
    -9.13E-03 -5.49E-03  5.88E-06  6.30E-03  1.17E-02
    -1.04E-02 -6.23E-03  6.65E-06  7.05E-03  1.30E-02
    -1.18E-02 -6.98E-03  7.36E-06  7.79E-03  1.43E-02
    -1.31E-02 -7.72E-03  8.16E-06  8.54E-03  1.56E-02
    -1.44E-02 -8.47E-03  8.87E-06  9.28E-03  1.69E-02
    -1.57E-02 -9.21E-03  9.64E-06  1.00E-02  1.82E-02
    -1.70E-02 -9.95E-03  1.04E-05  1.08E-02  1.96E-02
    -1.83E-02 -1.07E-02  1.11E-05  1.15E-02  2.09E-02
    -1.96E-02 -1.14E-02  1.18E-05  1.23E-02  2.22E-02
    -2.10E-02 -1.22E-02  1.26E-05  1.30E-02  2.35E-02];

% Aileron Effect - all derivative per deg
delta_ailLR_vec = [-40.0 -20.0 .0 20.0 40.0];
DCnda = [ -5.276E-04  -2.990E-04   0.000E+00   2.990E-04   5.276E-04
    -3.897E-05  -2.208E-05   0.000E+00   2.208E-05   3.897E-05
    4.472E-04   2.534E-04   0.000E+00  -2.534E-04  -4.472E-04
    9.555E-04   5.414E-04   0.000E+00  -5.414E-04  -9.555E-04
    1.482E-03   8.396E-04   0.000E+00  -8.396E-04  -1.482E-03
    2.021E-03   1.145E-03   0.000E+00  -1.145E-03  -2.021E-03
    2.569E-03   1.456E-03   0.000E+00  -1.456E-03  -2.569E-03
    3.121E-03   1.768E-03   0.000E+00  -1.768E-03  -3.121E-03
    3.607E-03   2.044E-03   0.000E+00  -2.044E-03  -3.607E-03
    3.977E-03   2.254E-03   0.000E+00  -2.254E-03  -3.977E-03
    4.254E-03   2.411E-03   0.000E+00  -2.411E-03  -4.254E-03
    4.420E-03   2.505E-03   0.000E+00  -2.505E-03  -4.420E-03
    4.240E-03   2.402E-03   0.000E+00  -2.402E-03  -4.240E-03
    2.792E-03   1.582E-03   0.000E+00  -1.582E-03  -2.792E-03
    4.502E-04   2.551E-04   0.000E+00  -2.551E-04  -4.502E-04
    -1.197E-03  -6.783E-04   0.000E+00   6.783E-04   1.197E-03
    -2.487E-03  -1.409E-03   0.000E+00   1.409E-03   2.487E-03
    -3.593E-03  -2.036E-03   0.000E+00   2.036E-03   3.593E-03
    -4.260E-03  -2.414E-03   0.000E+00   2.414E-03   4.260E-03
    -4.710E-03  -2.669E-03   0.000E+00   2.669E-03   4.710E-03 ];
DClda =  [-3.8748E-02 -2.1956E-02 0.0000E+00 2.1956E-02 3.8748E-02];

% Rudder Effect calculated AAA
DCldr   = 1.41e-4;  %deg-1
DCYdr   = 2.55e-3;  %deg-1
DCndr   = 8.97e-4;  %deg-1

%% ========================================================================
% Actuator parameters
%==========================================================================
wn_act = 44;        % natural frequency -- copy from HL20
z_act  = .7071;     % damping ratio
max_lim = [ 40  60  30 40]; % in order - aileron, elevator, rudder, flap
min_lim = [-40 -60 -30 0];

%% ========================================================================
% Throttle parameters
%==========================================================================
Thr2rpm_data = ((1+tanh([-3:3]))/(1+tanh(2.65))-0.0025)*9000;
Thr2rpm_breakpoints = (3+([-3:3]))/6;

rpm2force_data = [0 85 100];
rpm2force_breakpoints = [0 7000 9000];

%Load trim data from files
load VTP_trimcondition;
load VTP_ssmodes;
%initial vectors
xinco1=[OperatingPoint.op_point.States(1).x;OperatingPoint.op_point.States(2).x;OperatingPoint.op_point.States(3).x;...
    OperatingPoint.op_point.States(4).x(1);OperatingPoint.op_point.States(4).x(2);-OperatingPoint.op_point.States(4).x(3)];
xinco=[OperatingPoint.op_point.States(1).x;OperatingPoint.op_point.States(2).x;OperatingPoint.op_point.States(3).x;...
    OperatingPoint.op_point.States(4).x(1);OperatingPoint.op_point.States(4).x(2);OperatingPoint.op_point.States(4).x(3)];
Hdot0=0;
uinco=[OperatingPoint.op_point.Inputs(1).u,OperatingPoint.op_point.Inputs(2).u,OperatingPoint.op_point.Inputs(3).u,OperatingPoint.op_point.Inputs(4).u];
dTinco=OperatingPoint.op_point.Inputs(5).u;
%Load control params
control_params


